import rclpy
from rclpy.node import Node

import numpy as np
from scipy.spatial.transform import Rotation

from yyrobot_msg.msg import ArmGoalState
from rclpy.clock import Clock

import time

class DemoPublisher(Node):

    def __init__(self):
        super().__init__('minimal_publisher')
        self.publisher_ = self.create_publisher(ArmGoalState, '/BlRobotCtrl', 10)
        timer_period = 0.5  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0
        self.trigered=False

    def timer_callback(self):
        if self.trigered==True :
            return
            
        self.trigered=True

        self.demo1()
        #self.demo2()

    def demo1(self):
        
        # Box: x,y,z = 1,1,1
        
        #translation Matrix
        translation_matrix = np.array([0.7,0,0])

        # Rotate 45 by Y axis
        rotation_matrix45 = Rotation.from_euler('xyz',[0,-30,0],degrees=True)
        rotation_matrix45_arm = Rotation.from_euler('xyz',[0,60,0],degrees=True)
        
        step=0.1;
        angle=0

        r = 0.25
        z = 0.61
        
        while angle < 6.28 :
            x = r*np.sin( angle ) 
            y = r*np.cos( angle )
            Pn = np.array( [x,y,z] );
            #print ( Rn )
            Pn2 = rotation_matrix45.apply(Pn)
            
            Pn3 = translation_matrix + Pn2 
            Point_rm=  rotation_matrix45_arm.as_quat()
            #ation_matrix45.cop
            
            #print( Point_n )
            
            msg = ArmGoalState()
            time_stamp = Clock().now()
            msg.header.stamp = time_stamp.to_msg()
            msg.cmd = "plan" 
            msg.id  = "id"
            msg.goal_type = "W"
            msg.name = [""]
        
            msg.position = [Pn3[0],Pn3[1],Pn3[2],Point_rm[0],Point_rm[1],Point_rm[2],Point_rm[3]]
            #print( "!" )
            #print( msg.position )
            self.publisher_.publish(msg)
            #self.get_logger().info('Publishing: "%s"' % msg.cmd)
            self.i += 1
            angle = angle+step
            time.sleep(4)
        print( "End!" )

    def demo2(self):
        
        #Sphere: r = 0.5, p: x 0.8, z 0.4
        # angle: 45  r = 0.2
        
        sin45 = np.sin( np.radians( 45 ) )
        
        #translation Matrix
        translation_matrix = np.array([0.8,0,0.4])
        # Rotate 45 by Y axis
        #rotation_matrix45 = np.array([[sin45,0,sin45],[0,1,0],[-sin45,0,sin45]])
        rotation_matrix45 = Rotation.from_euler('xyz',[0,-45,0],degrees=True)
        #matrix(rotation_matrix45)
        
        step=0.1;
        angle=0
        r = (0.5 + 0.31) * 0.5
        z = (0.5 + 0.31) * 0.866
        while angle < 6.28 :
            x = r*np.sin( angle ) 
            y = r*np.cos( angle )
            Rn = np.array( [x,y,z] );
            #print ( Rn )
            Rn = rotation_matrix45.apply(Rn)
            #print("Rn")
            #print( Rn )
            
            Point_n = translation_matrix + Rn 
            Point_rm= self.get_rotation_matrix(-Rn)
            
            #print( Point_n )
            
            msg = ArmGoalState()
            time_stamp = Clock().now()
            msg.header.stamp = time_stamp.to_msg()
            msg.cmd = "plan" 
            msg.id  = "id"
            msg.goal_type = "W"
            msg.name = [""]
        
            msg.position = [Point_n[0],Point_n[1],Point_n[2],Point_rm[0],Point_rm[1],Point_rm[2],Point_rm[3]]
            #print( "!" )
            #print( msg.position )
            self.publisher_.publish(msg)
            #self.get_logger().info('Publishing: "%s"' % msg.cmd)
            self.i += 1
            angle = angle+step
            time.sleep(4)
        print( "End!" )

    def get_rotation_matrix(self,axis_input):
        axis = axis_input.copy()
        axis = np.asarray(axis)
        axis = axis / np.linalg.norm(axis)
    
        # 计算新坐标系的Y轴和Z轴
        if axis[0] != 0 or axis[1] != 0:
            y_axis = np.array([-axis[1], axis[0], 0])
        else:
            y_axis = np.array([0, -axis[2], axis[1]])
        z_axis = np.cross(axis, y_axis)
    
        # 构造旋转矩阵
        R = np.column_stack((axis, y_axis, z_axis))
    
        return Rotation.from_matrix(R).as_quat()
            

def main(args=None):
    rclpy.init(args=args)

    minimal_publisher = DemoPublisher()

    rclpy.spin(minimal_publisher)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
